#include "chuansu_can_adapter.h"
#include "mr418.h"
#include "sensor_utils.hpp"
#include "sr439.h"

using namespace sensor::radar;
using namespace sensor::utils;

ChuansuCanAdapter::ChuansuCanAdapter(std::unordered_map<std::string, std::shared_ptr<CanTransreceiver>> devs)
    : CanAdapter(devs, "ChuanSu")
{
    using namespace std::placeholders;
    message_unpack_table_ = {
        // { MR418_QUALITY_INFORMATION_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveMR418QualityInfo, this, _1, _2, _3) },
        { MR418_EXTENDED_INFORMATION_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveMR418ExtendInfo, this, _1, _2, _3) },
        { MR418_GENERAL_INFORMATION_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveMR418GeneralInfo, this, _1, _2, _3) },
        // { MR418_TRACK_LIST_STATUS_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveMR418TrackListStatus, this, _1, _2, _3) }
        { SR439_OBJ_EXTENDED_INFO_FL_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439ExtendFLInfo, this, _1, _2, _3) },
        { SR439_OBJ_EXTENDED_INFO_FR_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439ExtendFRInfo, this, _1, _2, _3) },
        //{ SR439_OBJ_EXTENDED_INFO_RL_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439ExtendRLInfo, this, _1, _2, _3) },
        //{ SR439_OBJ_EXTENDED_INFO_RR_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439ExtendRRInfo, this, _1, _2, _3) },
        { SR439_OBJ_GENERAL_INFO_FL_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439GeneralFLInfo, this, _1, _2, _3) },
        { SR439_OBJ_GENERAL_INFO_FR_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439GeneralFRInfo, this, _1, _2, _3) },
        //{ SR439_OBJ_GENERAL_INFO_RL_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439GeneralRLInfo, this, _1, _2, _3) },
        //{ SR439_OBJ_GENERAL_INFO_RR_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439GeneralRRInfo, this, _1, _2, _3) },
        { SR439_OBJ_QUALITY_INFO_FL_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439QualityFLInfo, this, _1, _2, _3) },
        { SR439_OBJ_QUALITY_INFO_FR_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439QualityFRInfo, this, _1, _2, _3) },
        //{ SR439_OBJ_QUALITY_INFO_RL_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439QualityRLInfo, this, _1, _2, _3) },
        //{ SR439_OBJ_QUALITY_INFO_RR_FRAME_ID, std::bind(&ChuansuCanAdapter::onReceiveSR439QualityRRInfo, this, _1, _2, _3) },
    };
    // auto pool1 = radar1LoopThread_->startLoop();
    // pool1->runEvery(0.1, std::bind(&ChuansuCanAdapter::onSendRadar1Handler, this));
    // auto pool2 = radar2LoopThread_->startLoop();
    // pool2->runEvery(0.1, std::bind(&ChuansuCanAdapter::onSendRadar2Handler, this));
}

std::unordered_map<int, int> ChuansuCanAdapter::chuansuObjectTypeMap = {
    //{ 0, 0 }, // 点
    { 1, 101 }, // 行人
    { 2, 1 }, // 汽车
    { 3, 5 }, // 卡车
    { 4, 102 }, // 自行车/摩托车
    //{ 5, 0 }, // 地面
    //{ 6, 0 }, // 高
};

double ChuansuCanAdapter::degreesToRadians(double degrees)
{
    return degrees * M_PI / 180.0;
}

void ChuansuCanAdapter::onReceiveCanMessage(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    // RCLCPP_INFO(logger_, "ChuanSu Rx:%s %04x %d %s", dev.c_str(), fid, buffer.readableBytes(),
    //     encodeToHexString(buffer.toStringPiece().as_string()).c_str());

    auto iter = message_unpack_table_.find(fid);
    if (iter == message_unpack_table_.end()) {
        // RCLCPP_WARN(logger_, "No Handle to unpack frame %04x", fid);
        return;
    }

    iter->second(dev, fid, buffer);
}

void ChuansuCanAdapter::onReceiveGnssMessage(const interface::msg::Gnss::SharedPtr)
{
}

void ChuansuCanAdapter::onReceiveInformationMessage(const interface::msg::CarInformation msg)
{
    // RCLCPP_INFO(logger_, "onReceiveInformationMessage start");
    static std::unordered_map<int, int> gearsCsMap = {
        { 0, 0 },
        { 1, 1 },
        { 2, 2 },
        { 3, 3 },
        { 4, 4 },
        { 5, 5 },
        { 6, 6 },
        { 7, 7 },
        { 8, 2 },
    };
    // 发送 300
    {
        sr439_veh_info1_t vehinfo1;
        vehinfo1.veh_speed_v = 0; // 有效
        vehinfo1.veh_speed_in = msg.speed; // 车速
        vehinfo1.yaw_rat_v = 0; // 有效
        vehinfo1.yaw_rate_in = degreesToRadians(msg.yawrate);
        vehinfo1.str_angle_v = 1;
        vehinfo1.str_angle = 0;
        uint8_t dstp[8];
        if (int ret = sr439_veh_info1_pack(dstp, &vehinfo1, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", SR439_VEH_INFO1_FRAME_ID, buffer);
        }
    }
    // 发送 301
    {
        sr439_veh_info2_t vehinfo2;
        vehinfo2.gear_state_now = gearsCsMap[msg.gearstate];
        vehinfo2.gear_sta_now_v = 1;
        vehinfo2.ign_state_v = 0;
        vehinfo2.ign_state = 0;
        uint8_t dstp[8];
        if (int ret = sr439_veh_info2_pack(dstp, &vehinfo2, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", SR439_VEH_INFO2_FRAME_ID, buffer);
        }
    }
    // RCLCPP_BEDUG(logger_, "onReceiveInformationMessage end");
}

void ChuansuCanAdapter::onReceiveMR418QualityInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    struct mr418_quality_information_t quality;
    auto piece = buffer.toStringPiece();
    int retv = mr418_quality_information_unpack(&quality, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]mr418_quality_information_unpack return %d", fid, retv);
}

void ChuansuCanAdapter::onReceiveMR418ExtendInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    struct mr418_extended_information_t extend;
    auto piece = buffer.toStringPiece();
    int retv = mr418_extended_information_unpack(&extend, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]mr418_extended_information_unpack return %d %d", fid, retv, extend.id);
    // 目标转objects
    interface::msg::Objects objects;
    radar_obj_pubs_["radar1"]->publish(objects);
}

void ChuansuCanAdapter::onReceiveMR418GeneralInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    struct mr418_general_information_t general;
    auto piece = buffer.toStringPiece();
    int retv = mr418_general_information_unpack(&general, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]mr418_general_information_unpack return %d %d", fid, retv, general.id);
}

void ChuansuCanAdapter::onReceiveMR418TrackListStatus(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    struct mr418_track_list_status_t track_list;
    auto piece = buffer.toStringPiece();
    int retv = mr418_track_list_status_unpack(&track_list, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]mr418_track_list_status_unpack return %d", fid, retv);
}

void ChuansuCanAdapter::onReceiveSR439ExtendFLInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    std::lock_guard<std::mutex> locker(radar1MapLock_);
    struct sr439_obj_extended_info_rl_t extend;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_extended_info_rl_unpack(&extend, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar1 [%04x]sr439_obj_extended_info_rl_unpack return %d %d", fid, retv, extend.obj_id);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (radar1ObjectsMap.find(extend.obj_id) != radar1ObjectsMap.end()) {
        object = radar1ObjectsMap[extend.obj_id].second;
        object.source = 2;
        object.type = chuansuObjectTypeMap[extend.obj_class];
        object.length = extend.obj_length * 0.1;
        object.height = extend.obj_height * 0.5;
        // object.speedx = extend.obj_arel_lat * 0.04;
        // object.speedy = extend.obj_arel_lon * 0.04;
        // object.speed = std::hypot(object.speedx, object.speedy);
        radar1ObjectsMap[extend.obj_id].first = now.microSecondsSinceEpoch();
    } else {
        object.source = 2;
        object.id = extend.obj_id;
        object.type = chuansuObjectTypeMap[extend.obj_class];
        object.length = extend.obj_length * 0.1;
        object.height = extend.obj_height * 0.5;
        // object.speedx = extend.obj_arel_lat * 0.04;
        // object.speedy = extend.obj_arel_lon * 0.04;
        // object.speed = std::hypot(object.speedx, object.speedy);
        auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
        radar1ObjectsMap[object.id] = obj;
    }
    std::cout << " 类别: " << (int)extend.obj_class << std::endl;
    std::cout << printObject(object).c_str() << std::endl;
}
void ChuansuCanAdapter::onReceiveSR439ExtendFRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    std::lock_guard<std::mutex> locker(radar2MapLock_);
    struct sr439_obj_extended_info_rl_t extend;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_extended_info_rl_unpack(&extend, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar2 [%04x]sr439_obj_extended_info_rl_unpack return %d %d", fid, retv, extend.obj_id);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (radar2ObjectsMap.find(extend.obj_id) != radar2ObjectsMap.end()) {
        object = radar2ObjectsMap[extend.obj_id].second;
        object.source = 2;
        object.type = chuansuObjectTypeMap[extend.obj_class];
        object.length = extend.obj_length * 0.1;
        object.height = extend.obj_height * 0.5;
        // object.speedx = extend.obj_arel_lat * 0.04;
        // object.speedy = extend.obj_arel_lon * 0.04;
        // object.speed = std::hypot(object.speedx, object.speedy);
        radar2ObjectsMap[extend.obj_id].first = now.microSecondsSinceEpoch();
    } else {
        object.source = 2;
        object.id = extend.obj_id;
        object.type = chuansuObjectTypeMap[extend.obj_class];
        object.length = extend.obj_length * 0.1;
        object.height = extend.obj_height * 0.5;
        // object.speedx = extend.obj_arel_lat * 0.04;
        // object.speedy = extend.obj_arel_lon * 0.04;
        // object.speed = std::hypot(object.speedx, object.speedy);
        auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
        radar2ObjectsMap[object.id] = obj;
    }
    std::cout << " 类别: " << (int)extend.obj_class << std::endl;
    std::cout << printObject(object).c_str() << std::endl;
}
void ChuansuCanAdapter::onReceiveSR439ExtendRLInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct sr439_obj_extended_info_rl_t extend;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_extended_info_rl_unpack(&extend, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]sr439_obj_extended_info_rl_unpack return %d %d", fid, retv, extend.obj_id);
}
void ChuansuCanAdapter::onReceiveSR439ExtendRRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    std::lock_guard<std::mutex> locker(radar3MapLock_);
    struct sr439_obj_extended_info_rl_t extend;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_extended_info_rl_unpack(&extend, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar4 [%04x]sr439_obj_extended_info_rl_unpack return %d %d", fid, retv, extend.obj_id);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (radar3ObjectsMap.find(extend.obj_id) != radar3ObjectsMap.end()) {
        object = radar3ObjectsMap[extend.obj_id].second;
        object.source = 2;
        object.type = chuansuObjectTypeMap[extend.obj_class];
        object.length = extend.obj_length * 0.1;
        object.height = extend.obj_height * 0.5;
        // object.speedx = extend.obj_arel_lat * 0.04;
        // object.speedy = extend.obj_arel_lon * 0.04;
        // object.speed = std::hypot(object.speedx, object.speedy);
        radar3ObjectsMap[extend.obj_id].first = now.microSecondsSinceEpoch();
    } else {

        object.source = 2;
        object.id = extend.obj_id;
        object.type = chuansuObjectTypeMap[extend.obj_class];
        object.length = extend.obj_length * 0.1;
        object.height = extend.obj_height * 0.5;
        // object.speedx = extend.obj_arel_lat * 0.04;
        // object.speedy = extend.obj_arel_lon * 0.04;
        // object.speed = std::hypot(object.speedx, object.speedy);
        auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
        radar3ObjectsMap[object.id] = obj;
    }
}
void ChuansuCanAdapter::onReceiveSR439QualityFLInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct sr439_obj_quality_info_rl_t quality;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_quality_info_rl_unpack(&quality, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar1 [%04x]sr439_obj_quality_info_rl_unpack return %d %d", fid, retv, quality.obj_id);
}
void ChuansuCanAdapter::onReceiveSR439QualityFRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct sr439_obj_quality_info_rl_t quality;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_quality_info_rl_unpack(&quality, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar2 [%04x]sr439_obj_quality_info_rl_unpack return %d %d", fid, retv, quality.obj_id);
    // 目标转objects
    // interface::msg::Objects objects;
    // radar_obj_pubs_["radar2"]->publish(objects);
}
void ChuansuCanAdapter::onReceiveSR439QualityRLInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct sr439_obj_quality_info_rl_t quality;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_quality_info_rl_unpack(&quality, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]sr439_obj_quality_info_rl_unpack return %d %d", fid, retv, quality.obj_id);
}
void ChuansuCanAdapter::onReceiveSR439QualityRRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct sr439_obj_quality_info_rl_t quality;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_quality_info_rl_unpack(&quality, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]sr439_obj_quality_info_rl_unpack return %d %d", fid, retv, quality.obj_id);
}

void ChuansuCanAdapter::onReceiveSR439GeneralFLInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    std::lock_guard<std::mutex> locker(radar1MapLock_);
    struct sr439_obj_general_info_rl_t general;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_general_info_rl_unpack(&general, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar1 [%04x]sr439_obj_general_info_rl_unpack return %d %d", fid, retv, general.obj_id);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (radar1ObjectsMap.find(general.obj_id) != radar1ObjectsMap.end()) {
        object = radar1ObjectsMap[general.obj_id].second;
        object.x = general.obj_dist_long * 0.0625;
        object.y = general.obj_dist_lat * 0.0078;
        object.z = 0.0;
        object.speedx = general.obj_vrel_long * 0.05;
        object.speedy = general.obj_vrel_lat * 0.05;
        object.speed = std::hypot(object.speedx, object.speedy);
        radar1ObjectsMap[general.obj_id].first = now.microSecondsSinceEpoch();
    } else {
        object.x = general.obj_dist_long * 0.0625;
        object.y = general.obj_dist_lat * 0.0078;
        object.z = 0.0;
        object.speedx = general.obj_vrel_long * 0.05;
        object.speedy = general.obj_vrel_lat * 0.05;
        object.speed = std::hypot(object.speedx, object.speedy);
        auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
        radar1ObjectsMap[object.id] = obj;
    }
    std::cout << printObject(object).c_str() << std::endl;
}
void ChuansuCanAdapter::onReceiveSR439GeneralFRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    std::lock_guard<std::mutex> locker(radar2MapLock_);
    struct sr439_obj_general_info_rl_t general;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_general_info_rl_unpack(&general, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar2 [%04x]sr439_obj_general_info_rl_unpack return %d %d", fid, retv, general.obj_id);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (radar2ObjectsMap.find(general.obj_id) != radar2ObjectsMap.end()) {
        object = radar2ObjectsMap[general.obj_id].second;
        object.x = general.obj_dist_long * 0.0625;
        object.y = general.obj_dist_lat * 0.0078;
        object.z = 0.0;
        object.speedx = general.obj_vrel_long * 0.05;
        object.speedy = general.obj_vrel_lat * 0.05;
        object.speed = std::hypot(object.speedx, object.speedy);
        radar2ObjectsMap[general.obj_id].first = now.microSecondsSinceEpoch();
    } else {
        object.x = general.obj_dist_long * 0.0625;
        object.y = general.obj_dist_lat * 0.0078;
        object.z = 0.0;
        object.speedx = general.obj_vrel_long * 0.05;
        object.speedy = general.obj_vrel_lat * 0.05;
        object.speed = std::hypot(object.speedx, object.speedy);
        auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
        radar2ObjectsMap[object.id] = obj;
    }
    std::cout << printObject(object).c_str() << std::endl;
}
void ChuansuCanAdapter::onReceiveSR439GeneralRLInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct sr439_obj_general_info_rl_t general;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_general_info_rl_unpack(&general, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]sr439_obj_general_info_rl_unpack return %d %d", fid, retv, general.obj_id);
    // 目标转objects
    // interface::msg::Objects objects;
    // radar_obj_pubs_["radar1"]->publish(objects);
}
void ChuansuCanAdapter::onReceiveSR439GeneralRRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct sr439_obj_general_info_rl_t general;
    auto piece = buffer.toStringPiece();
    int retv = sr439_obj_general_info_rl_unpack(&general, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "[%04x]sr439_obj_general_info_rl_unpack return %d %d", fid, retv, general.obj_id);
    // 目标转objects
    // interface::msg::Objects objects;
    // radar_obj_pubs_["radar1"]->publish(objects);
}

void ChuansuCanAdapter::onSendRadar1Handler()
{
    auto now = muduo::Timestamp::now();
    std::lock_guard<std::mutex> locker(radar1MapLock_);
    uint64_t time = now.microSecondsSinceEpoch();
    interface::msg::Objects objes;
    objes.header.stamp.sec = now.secondsSinceEpoch();
    objes.header.stamp.nanosec = (time % 1000000) * 1000;
    objes.header.frame_id = "radar1";
    for (auto it = radar1ObjectsMap.begin(); it != radar1ObjectsMap.end();) {
        if (time - it->second.first > 200) {
            it = radar1ObjectsMap.erase(it);
        } else {
            std::cout << printObject(it->second.second) << std::endl;
            objes.objects.emplace_back(it->second.second);
            ++it;
        }
    }
    radar_obj_pubs_[objes.header.frame_id]->publish(objes);
}
void ChuansuCanAdapter::onSendRadar2Handler()
{
    auto now = muduo::Timestamp::now();
    std::lock_guard<std::mutex> locker(radar2MapLock_);
    uint64_t time = now.microSecondsSinceEpoch();
    interface::msg::Objects objes;
    objes.header.stamp.sec = now.secondsSinceEpoch();
    objes.header.stamp.nanosec = (time % 1000000) * 1000;
    objes.header.frame_id = "radar2";
    for (auto it = radar2ObjectsMap.begin(); it != radar2ObjectsMap.end();) {
        if (time - it->second.first > 200) {
            it = radar2ObjectsMap.erase(it);
        } else {
            std::cout << printObject(it->second.second) << std::endl;
            objes.objects.emplace_back(it->second.second);
            ++it;
        }
    }
    radar_obj_pubs_[objes.header.frame_id]->publish(objes);
}

std::string ChuansuCanAdapter::printObject(const interface::msg::Object& object)
{
    std::stringstream ss;

    ss << "frameId:" << object.header.frame_id << ",id:" << object.id << ",x:" << object.x << ",y:" << object.y << ",type:" << object.type
       << ",heading:" << object.heading << ",speed:" << object.speed << ",speedX:" << object.speedx << ",speedY:" << object.speedy;

    return ss.str();
}